#include "Map.h"

#include <math.h>

using namespace std;

namespace opennav {

Map::Map(FileInterface* input, bool enablecache) :
	input_(input), enablecache_(enablecache) {
	reader_ = new MapReader(input_, this);
	reader_->zoom_table_entries(zoomlevels_);
	reader_->map_tile_def_table_entries(tiledefs_);

	route_ = NULL;
	router_ = new MapRouter(this);
}

Map::~Map() {
	delete_route();
	delete router_;
	flush_tilecache();
	//TODO: delete all mapobjects, gerade keine Lust nachzudenken
}

uint8_t Map::get_zoom_level(const uint8_t lod) {
	uint8_t zoom_level = (*(zoomlevels_.begin())).zoom_level;
	for (zoomlevels::iterator it = zoomlevels_.begin(); it != zoomlevels_.end(); it++) {
		if ((*it).resolution > lod)
			break;
		zoom_level = (*it).zoom_level;
	}
	return zoom_level;
}

void Map::create_selection(const Area& bounds, uint8_t lod,
		MapSelection& map_selection) {

	uint8_t zoom_level = get_zoom_level(lod);

	vector<MapTileDef> tiledefs;
	select_tiles(bounds, zoom_level, 0, tiledefs);

	map_selection.map = this;
	map_selection.bounds = bounds;
	map_selection.items.clear();
	for (vector<MapTileDef>::iterator it = tiledefs.begin(); it
			!= tiledefs.end(); it++) {
		vector<MapObject*>* tile;

		get_tile_content(*it, tile);

		for (vector<MapObject*>::iterator obj = tile->begin(); obj
				!= tile->end(); obj++) {
			if ((*obj)->bounds.intersects(bounds)) {
				map_selection.items.push_back((*obj));
			}
		}
	}
}

const std::string Map::get_object_label(MapObject* obj) {
	int32_t label_offset = -1;
	std::string label;
	switch (obj->type()) {
	case LINE:
		if (((MapLine*) obj)->road == true) {
			Road road;
			reader_->road_road_entry(((MapLine*) obj)->reference, road);
			if (road.labels.size() > 0) {
				label = road.labels.at(0);
			}
		}
		break;
	case POINT:
		if (((MapPoint*) obj)->poi == false) {
			label_offset = ((MapPoint*) obj)->reference;
		}
		break;
	case SHAPE:
		label_offset = ((MapShape*) obj)->reference;
		break;
	case UNKNOWN:
		// not possible
		break;
	}
	if (label_offset >= 0) {
		if (enablecache_) {
			labelcache::iterator cache_it = labelcache_.find(label_offset);
			if (cache_it != labelcache_.end()) {
				label = cache_it->second;
			} else {
				label = reader_->load_label(label_offset);
				;
				labelcache_.insert(labelcache::value_type(label_offset, label));
			}
		} else {
			label = reader_->load_label(label_offset);
			;
		}
	}
	return label;
}

void Map::select_tiles(const Area& bounds, uint8_t zoom_level, uint16_t from,
		vector<MapTileDef>& tile_defs) {
	MapTileDef def;
	do {
		def = tiledefs_.at(from++);
		if (def.bounds.intersects(bounds)) {
			if (def.zoom.zoom_level == zoom_level) {
				tile_defs.push_back(def);
			} else {
				select_tiles(bounds, zoom_level, def.first_subtile_id,
						tile_defs);
			}
		}
	} while (def.last == false);
}

void Map::search_country(const std::string& name, SearchCountryResult& result) {
	int32_t middle, left = 0, right =
			reader_->search_header().country_tables_size - 1;
	bool found = false;
	{
		while (right >= left && found == false) {
			middle = left + ((right - left) / 2);
			SortedCountryTableEntry entry;
			reader_->search_sorted_country_table_entry(middle, entry);
			Country country;
			reader_->location_country_entry(entry.country_id, country);
			if (country.name.compare(0, name.size(), name) == 0) {
				found = true;
			} else if (country.name.compare(0, name.size(), name) > 0) {
				right = middle - 1;
			} else {
				left = middle + 1;
			}
		}
	}
	if (found) {
		int32_t from = middle, middleFrom, leftFrom = left, rightFrom = middle
				- 1;
		while (rightFrom >= leftFrom) {
			middleFrom = leftFrom + ((rightFrom - leftFrom) / 2);
			SortedCountryTableEntry entry;
			reader_->search_sorted_country_table_entry(middleFrom, entry);
			Country country;
			reader_->location_country_entry(entry.country_id, country);
			if (country.name.compare(0, name.size(), name) == 0) {
				from = middleFrom;
				rightFrom = middleFrom - 1;
			} else if (name.compare(0, country.name.size(), country.name) < 0) {
				rightFrom = middleFrom - 1;
			} else {
				leftFrom = middleFrom + 1;
			}
		}
		int32_t to = middle, middleTo, leftTo = middle + 1, rightTo = right;
		while (rightTo >= leftTo) {
			middleTo = leftTo + ((rightTo - leftTo) / 2);
			SortedCountryTableEntry entry;
			reader_->search_sorted_country_table_entry(middleTo, entry);
			Country country;
			reader_->location_country_entry(entry.country_id, country);
			if (country.name.compare(0, name.size(), name) == 0) {
				to = middleTo;
				leftTo = middleTo + 1;
			} else if (country.name.compare(0, name.size(), name) > 0) {
				rightTo = middleTo - 1;
			} else {
				leftTo = middleTo + 1;
			}
		}
		result.range.count = (to - from) + 1;
		result.range.start = from;
	} else {
		result.range.count = 0;
		result.range.start = -1;
	}
}

void Map::get_result(int32_t n, SearchCountryResult& result, Country& country) {
	SortedCountryTableEntry entry;
	reader_->search_sorted_country_table_entry(result.range.at(n), entry);
	reader_->location_country_entry(entry.country_id, country);
}

void Map::search_city(const std::string& name, uint8_t country_id,
		SearchCityResult& result) {
	int32_t middle;
	CountryToCityTableEntry ctcte;
	reader_->search_country_to_city_table_entry(country_id, ctcte);
	int32_t left = ctcte.sorted_city_table_entry_id;
	int32_t right = reader_->search_header().city_tables_size - 1;
	if (country_id < reader_->search_header().country_tables_size - 1) {
		CountryToCityTableEntry ctcte2;
		reader_->search_country_to_city_table_entry(country_id + 1, ctcte2);
		right = ctcte2.sorted_city_table_entry_id - 1;
	}
	bool found = false;
	{
		while (right >= left && found == false) {
			middle = left + ((right - left) / 2);
			SortedCityTableEntry entry;
			reader_->search_sorted_city_table_entry(middle, entry);
			City city;
			reader_->location_city_entry(entry.city_id, city);
			if (city.name.compare(0, name.size(), name) == 0) {
				found = true;
			} else if (city.name.compare(0, name.size(), name) > 0) {
				right = middle - 1;
			} else {
				left = middle + 1;
			}
		}
	}
	if (found) {
		int32_t from = middle, middleFrom, leftFrom = left, rightFrom = middle
				- 1;
		while (rightFrom >= leftFrom) {
			middleFrom = leftFrom + ((rightFrom - leftFrom) / 2);
			SortedCityTableEntry entry;
			reader_->search_sorted_city_table_entry(middleFrom, entry);
			City city;
			reader_->location_city_entry(entry.city_id, city);
			if (city.name.compare(0, name.size(), name) == 0) {
				from = middleFrom;
				rightFrom = middleFrom - 1;
			} else if (name.compare(0, city.name.size(), city.name) < 0) {
				rightFrom = middleFrom - 1;
			} else {
				leftFrom = middleFrom + 1;
			}
		}
		int32_t to = middle, middleTo, leftTo = middle + 1, rightTo = right;
		while (rightTo >= leftTo) {
			middleTo = leftTo + ((rightTo - leftTo) / 2);
			SortedCityTableEntry entry;
			reader_->search_sorted_city_table_entry(middleTo, entry);
			City city;
			reader_->location_city_entry(entry.city_id, city);
			if (city.name.compare(0, name.size(), name) == 0) {
				to = middleTo;
				leftTo = middleTo + 1;
			} else if (city.name.compare(0, name.size(), name) > 0) {
				rightTo = middleTo - 1;
			} else {
				leftTo = middleTo + 1;
			}
		}
		result.range.count = (to - from) + 1;
		result.range.start = from;
	} else {
		result.range.count = 0;
		result.range.start = -1;
	}
}

void Map::get_result(int32_t n, SearchCityResult& result, City& city) {
	SortedCityTableEntry entry;
	reader_->search_sorted_city_table_entry(result.range.at(n), entry);
	reader_->location_city_entry(entry.city_id, city);
}

void Map::search_road(const std::string& name, int32_t city_id,
		SearchRoadResult& result) {
	int32_t current_city_id = city_id;
	while (current_city_id < reader_->location_header().city_table_size) {
		if (current_city_id != city_id) {
			City subcity;
			reader_->location_city_entry(current_city_id, subcity);
			if (subcity.parent_city_id < city_id) {
				break;
			}
		}
		int32_t middle;
		CityToRoadTableEntry ctrte;
		reader_->search_city_to_road_table_entry(current_city_id, ctrte);
		int32_t left = ctrte.sorted_road_table_entry_id;
		int32_t right = reader_->search_header().road_tables_size - 1;
		if (current_city_id < reader_->search_header().city_tables_size - 1) {
			CityToRoadTableEntry ctrte2;
			reader_->search_city_to_road_table_entry(current_city_id + 1,
					ctrte2);
			right = ctrte2.sorted_road_table_entry_id - 1;
		}
		bool found = false;
		{
			while (right >= left && found == false) {
				middle = left + ((right - left) / 2);
				SortedRoadTableEntry entry;
				reader_->search_sorted_road_table_entry(middle, entry);
				Road road;
				reader_->road_road_entry(entry.road_id, road);
				uint8_t label_id = entry.label_id;
				if (road.labels.at(label_id).compare(0, name.size(), name) == 0) {
					found = true;
				} else if (road.labels.at(label_id).compare(0, name.size(),
						name) > 0) {
					right = middle - 1;
				} else {
					left = middle + 1;
				}
			}
		}
		if (found) {
			int32_t from = middle, middleFrom, leftFrom = left, rightFrom =
					middle - 1;
			while (rightFrom >= leftFrom) {
				middleFrom = leftFrom + ((rightFrom - leftFrom) / 2);
				SortedRoadTableEntry entry;
				reader_->search_sorted_road_table_entry(middleFrom, entry);
				Road road;
				reader_->road_road_entry(entry.road_id, road);
				uint8_t label_id = entry.label_id;
				if (road.labels.at(label_id).compare(0, name.size(), name) == 0) {
					from = middleFrom;
					rightFrom = middleFrom - 1;
				} else if (name.compare(0, road.labels.at(label_id).size(),
						road.labels.at(label_id)) < 0) {
					rightFrom = middleFrom - 1;
				} else {
					leftFrom = middleFrom + 1;
				}
			}
			int32_t to = middle, middleTo, leftTo = middle + 1, rightTo = right;
			while (rightTo >= leftTo) {
				middleTo = leftTo + ((rightTo - leftTo) / 2);
				SortedRoadTableEntry entry;
				reader_->search_sorted_road_table_entry(middleTo, entry);
				Road road;
				reader_->road_road_entry(entry.road_id, road);
				uint8_t label_id = entry.label_id;
				if (road.labels.at(label_id).compare(0, name.size(), name) == 0) {
					to = middleTo;
					leftTo = middleTo + 1;
				} else if (road.labels.at(label_id).compare(0, name.size(),
						name) > 0) {
					rightTo = middleTo - 1;
				} else {
					leftTo = middleTo + 1;
				}
			}
			TableSection section;
			section.count = (to - from) + 1;
			section.start = from;
			result.range.sections.push_back(section);
		}
		current_city_id++;
	}
}

void Map::get_result(int32_t n, SearchRoadResult& result, Road& road) {
	SortedRoadTableEntry entry;
	reader_->search_sorted_road_table_entry(result.range.at(n), entry);
	reader_->road_road_entry(entry.road_id, road);
}

void Map::country(uint8_t country_id, Country& country) {
	reader_->location_country_entry(country_id, country);
}

void Map::region(int16_t region_id, Region& region) {
	reader_->location_region_entry(region_id, region);
}

void Map::city(int32_t city_id, City& city) {
	reader_->location_city_entry(city_id, city);
}

void Map::get_segment(int32_t offset, RoadSegment& segment) {
	reader_->road_segment_entry(offset, segment);
}

void Map::get_road(const RoadSegment& segment, Road& road){
	reader_->road_road_entry(segment.road_id, road);
}

void Map::get_junction(int32_t offset, RoadJunction& junction) {
	reader_->road_junction_entry(offset, junction);
}

void Map::get_tile_content(const MapObjectReference& reference, std::vector<
		MapObject*>*& tile_content) {
	MapTileDef tile_def;
	reader_->map_tile_def_table_entry(reference.tile_id, tile_def);
	get_tile_content(tile_def, tile_content);
}

void Map::get_tile_content(const MapTileDef& tile_def,
		std::vector<MapObject*>*& tile_content) {
	if (enablecache_) {
		tilecache::iterator cache_it = tilecache_.find(tile_def.tile_id);
		if (cache_it != tilecache_.end()) {
			tile_content = cache_it->second;
		} else {
			tile_content = new std::vector<MapObject*>();
			reader_->map_tile_table_entry(tile_def, *tile_content);
			tilecache_.insert(tilecache::value_type(tile_def.tile_id,
					tile_content));
		}
	} else {
		tile_content = new std::vector<MapObject*>();
		reader_->map_tile_table_entry(tile_def, *tile_content);
	}
}

void Map::flush_tilecache() {
	if (enablecache_) {
		for (tilecache::iterator it = tilecache_.begin(); it
				!= tilecache_.end(); it++) {
			tilecache_.erase(it);
			delete it->second;
		}
	}
}

bool Map::get_map_object(const MapObjectReference& reference, MapLine& object) {
	std::vector<MapObject*>* tile_content;

	get_tile_content(reference, tile_content);

	MapObject* ob = tile_content->at(reference.object_id);
	if (ob->type() == LINE /* && ob->map_object_ref == reference */) {
		object = static_cast<MapLine&> (*ob);
		return true;
	}
	return false;
}

bool Map::get_map_object(const MapObjectReference& reference, MapPoint& object) {
	std::vector<MapObject*>* tile_content;

	get_tile_content(reference, tile_content);

	MapObject* ob = tile_content->at(reference.object_id);
	if (ob->type() == POINT /* && ob->map_object_ref == reference */) {
		object = static_cast<MapPoint&> (*ob);
		return true;
	}
	return false;
}

bool Map::get_map_object(const MapObjectReference& reference, MapShape& object) {
	std::vector<MapObject*>* tile_content;

	get_tile_content(reference, tile_content);

	MapObject* ob = tile_content->at(reference.object_id);
	if (ob->type() == SHAPE /* && ob->map_object_ref == reference */) {
		object = static_cast<MapShape&> (*ob);
		return true;
	}
	return false;
}

void Map::set_routing_settings(const RoutingSettings& settings) {
	router_->set_routing_settings(settings);
}

void Map::reset_route() {
	delete_route();
	route_ = new Route();
}

void Map::delete_route() {
	if (route_ != NULL) {
		delete route_;
		route_ = NULL;
	}
}

const Route* Map::get_route() const {
	return route_;
}

bool Map::calculate_route(const RoadJunction& src, const RoadJunction& dst) {
	reset_route();
	return calculate_route(src, dst, *route_);
}

bool Map::calculate_route(const RoadJunction& src, const RoadJunction& dst,
		Route& route) {
	if (router_->find(src, dst, route) < 0) {
		delete_route();
		return false;
	}
	return true;
}

bool Map::calculate_route(const Coordinate& src, const Coordinate& dst) {
	RoadJunction src_junc, dst_junc;
	Road src_road, dst_road;
	RoadSegment src_sgmt, dst_sgmt;
	Coordinate src_coord, dst_coord;
	if (reverse_geocode(src, src_road, src_sgmt, src_junc, src_coord) &&
			reverse_geocode(dst, dst_road, dst_sgmt, dst_junc, dst_coord)) {
		return calculate_route(src_junc, dst_junc);
	}
	return false;
}

bool Map::reverse_geocode(const Coordinate& coordinate, Road& road, RoadSegment& segment, RoadJunction& next_junction, Coordinate& c) {
	Area bounds(coordinate, 65536, 65536);
	vector<MapTileDef> tiledefs;
	select_tiles(bounds, zoomlevels_.at(zoomlevels_.size() - 1).zoom_level, 0, tiledefs);
	int32_t closest_distance = INT32_MAX;
	MapLine *closest = NULL;
	int x = 0, y = 0;
	for (vector<MapTileDef>::iterator it = tiledefs.begin(); it != tiledefs.end(); it++) {
		vector<MapObject*>* tile;
		get_tile_content(*it, tile);
		for (vector<MapObject*>::iterator obj = tile->begin(); obj
				!= tile->end(); obj++) {
			MapObject* object = *obj;
			if (object->type() != LINE) {
				continue;
			}
			x++;
			MapLine *line = static_cast<MapLine*> (object);
			if (!line->road) {
				continue;
			}
			if (!line->bounds.intersects(bounds)) {
				continue;
			}
			y++;
			for (uint32_t i = 1; i < line->coordinates.size(); i++) {
				int32_t ax = line->coordinates[i - 1].longitude;
				int32_t ay = line->coordinates[i - 1].latitude;
				int32_t bx = line->coordinates[i].longitude;
				int32_t by = line->coordinates[i].latitude;
				int32_t dx = bx - ax;
				int32_t dy = by - ay;
				int32_t dd = dx * dx + dy * dy;
				int32_t s = dx * (coordinate.longitude - ax) + dy * (coordinate.latitude - ay);
				int32_t distance = 0;
				Coordinate point;
				if (s >= dd) {
					distance = (bx - coordinate.longitude) * (bx - coordinate.longitude) + (by - coordinate.latitude) * (by - coordinate.latitude);
					point.longitude = bx;
					point.latitude = by;
				} else if (s <= 0) {
					distance = (ax - coordinate.longitude) * (ax - coordinate.longitude) + (ay - coordinate.latitude) * (ay - coordinate.latitude);
					point.longitude = ax;
					point.latitude = ay;
				} else {
					int64_t lon = (int64_t)ax + (int64_t)dx * (int64_t)s / (int64_t)dd;
					int64_t lat = (int64_t)ay + (int64_t)dy * (int64_t)s / (int64_t)dd;
					point.longitude = (int32_t)lon;
					point.latitude = (int32_t)lat;
					distance = (point.longitude - coordinate.longitude) * (point.longitude - coordinate.longitude) + (point.latitude - coordinate.latitude) * (point.latitude - coordinate.latitude);
				}
				if (distance < 0) distance = -distance;
				if (closest_distance > distance) {
					closest_distance = distance;
					c = point;
					closest = line;
				}
			}
		}
	}
	if (closest != NULL) {
		reader_->road_road_entry(closest->reference, road);
		std::vector<RoadSegment> segments;
		reader_->road_segment_entries(road.segments_offset, segments);
		for (std::vector<RoadSegment>::iterator segment_it =
				segments.begin(); segment_it != segments.end(); segment_it++) {
			for (std::vector<MapObjectReference>::const_iterator ref_it = (*segment_it).map_object_refs.begin();
					ref_it != (*segment_it).map_object_refs.end(); ref_it++) {
				if ((*ref_it) == closest->map_object_ref) {
					segment = (*segment_it);
					reader_->road_junction_entry(segment.junction_from_offset, next_junction);
					if (next_junction.coordinate == coordinate){
						return true;
					}
					if (segment.oneway) {
						reader_->road_junction_entry(segment.junction_to_offset, next_junction);
						return true;
					}
					RoadJunction from, to;
					reader_->road_junction_entry(segment.junction_from_offset, from);
					reader_->road_junction_entry(segment.junction_to_offset, to);
					int64_t d1 = (from.coordinate.longitude - c.longitude) *
							(from.coordinate.longitude - c.longitude) +
							(from.coordinate.latitude - c.latitude) *
							(from.coordinate.latitude - c.latitude);
					int64_t d2 = (to.coordinate.longitude - c.longitude) *
							(to.coordinate.longitude - c.longitude) +
							(to.coordinate.latitude - c.latitude) *
							(to.coordinate.latitude - c.latitude);
					next_junction = (d1 > d2) ? to : from;
					return true;
				}
			}
		}
		// ERROR!!!!!
		return false;
	}
	else {
		return false;
	}
}

bool Map::geocode(const Road& road, Coordinate& coordinate) {
	RoadSegment segment;
	reader_->road_segment_entry(road.segments_offset, segment);
	RoadJunction junction;
	reader_->road_junction_entry(segment.junction_from_offset, junction);
	coordinate = junction.coordinate;
	return true;
}

bool Map::geocode(const City& city, Coordinate& coordinate) {
	coordinate.longitude = 0;
	coordinate.latitude = 0;
	// TODO: implementation
	return false;
}

}
